# include "rclcpp/rclcpp.hpp"
# include "chapt4_interfaces/srv/patrol.hpp"
# include <chrono>
# include <ctime>
# include "rcl_interfaces/msg/parameter.hpp"
# include "rcl_interfaces/msg/parameter_value.hpp"
# include "rcl_interfaces/msg/parameter_type.hpp"
# include "rcl_interfaces/srv/set_parameters.hpp"

using SetParameter = rcl_interfaces::srv::SetParameters;
using Patrol = chapt4_interfaces::srv::Patrol;
using namespace std::chrono_literals; // 引入chrono_literals可以使用10s

class PatrollingClient : public rclcpp::Node
{
private:
    rclcpp::Client<Patrol>::SharedPtr client_;   
    rclcpp::TimerBase::SharedPtr timer_; // 定义一个定时器 
public:

    PatrollingClient() : Node("patrolling_client")
    {   
        srand(time(NULL));
        client_ = this->create_client<Patrol>("patrol");
        timer_ = this->create_wall_timer(10s, [&]()->void{
        while (!client_->wait_for_service(1s))
        {       
            RCLCPP_INFO(this->get_logger(), "等待服务。。。");
            if(!rclcpp::ok())
            {
                RCLCPP_ERROR(this->get_logger(), "rclcpp挂了");
                return;
            }

        }
        // 构造请求
        auto request = std::make_shared<Patrol::Request>();
        request ->target_x = rand()%15; // 随机数
        request ->target_y = rand()%15; // 随机数
        RCLCPP_INFO(this->get_logger(), "正在发送请求(%.2f,%.2f)",request->target_x,request->target_y);
        // 发送请求
        this -> client_->async_send_request(request,
        [&](rclcpp::Client<Patrol>::SharedFuture result)->void
        {
            auto response = result.get();
            if(response->result == Patrol::Response::SUCCESS)
            {
                RCLCPP_INFO(this->get_logger(), "可以到达目标点");
            }
            if(response->result == Patrol::Response::FAIL){
                RCLCPP_INFO(this->get_logger(), "不能到达目标点");
            }
        }
        );
        });
        
   }
    SetParameter::Response::SharedPtr call_set_parameter(rcl_interfaces::msg::Parameter &param)
    {
        auto parameter_client = this->create_client<SetParameter>("/turtle_control/set_parameters");
        while (!parameter_client->wait_for_service(1s))
        {
            RCLCPP_INFO(this->get_logger(), "Waiting for service...");
            if (!rclcpp::ok())
            {
                RCLCPP_ERROR(this->get_logger(), "rclcpp挂了");
                return nullptr;
            }
        }
        auto request = std::make_shared<SetParameter::Request>();
        // 填充数据
        request->parameters.push_back(param);
        // 发送请求
        auto future = parameter_client ->async_send_request(request);
        rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
        auto response = future.get();
        return response;

    } 
    void update_server_param_k(double k)
    {
        //.创建参数对象
        auto param = rcl_interfaces::msg::Parameter();
        param.name = "k";
        //.创建参数值
        auto param_value = rcl_interfaces::msg::ParameterValue();
        param_value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
        param_value.double_value = k;
        param.value = param_value;
        //.请求更新参数
        auto response = this->call_set_parameter(param);
        if(response == NULL)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to update parameter");
            return; 

        }
        for(auto result:response->results)
        {
            if(result.successful)
            {
                RCLCPP_INFO(this->get_logger(), "Successfully updated parameter");
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to update parameter reason is %s", result.reason.c_str());
            }

        }
    }

};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<PatrollingClient>();
  node->update_server_param_k(2.0);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}